#include "stm32f4xx_hal.h"
#include "main.h"
#include "printf.h"
#include "tim.h"
#include "ctrl.h"

/*define dead zone */

#define DEAD_LEN	1450
#define DEAD_LEN_LEN	450

#define DEAD_MB_H	1550
#define DEAD_MB_L	1450
#define DEAD_LR_H	1550
#define DEAD_LR_L	1450
#define DEAD_MB_LEN	450
#define DEAD_LR_LEN	450

#define DEAD_MID_LEN	50

#define DUTY_CYCLE_RANGE	1000
#define STOP_SPEED			2000

#define OP_NAME				0
#define OP_LEFT				1
#define OP_RIGHT			2

ch_pwm ch_pwm_value;

#define CTRL_LIST_NUM		17
#define CTRL_LIST_LEN		3
const uint8_t ctrl_list[CTRL_LIST_NUM][CTRL_LIST_LEN] = { { Stop, stop, stop },
		{ Go, forward, forward }, { Back, backward, backward }, { Left,
				backward, forward }, { Right, forward, backward }, { Go_R_L,
				forward, forward }, { Go_R_H, forward, backward }, { Go_R_M,
				forward, stop }, { Go_L_L, forward, forward }, { Go_L_H,
				backward, forward }, { Go_L_M, stop, forward }, { Back_R_L,
				backward, backward }, { Back_R_H, forward, backward }, {
				Back_R_M, stop, backward }, { Back_L_L, backward, backward }, {
				Back_L_H, backward, forward }, { Back_L_M, backward, stop }, };
ctrl_collection now_operate;
ctrl_collection last_operate;

//double rx_pwm_mb, double rx_pwm_lr
typedef uint8_t (*run_func)(void);
run_func run_fun_list[] = { do_Stop, do_Go, do_Back, do_Left, do_Right,
		do_Go_R_L, do_Go_R_H, do_Go_R_M, do_Go_L_L, do_Go_L_H, do_Go_L_M,
		do_Back_R_L, do_Back_R_H, do_Back_R_M, do_Back_L_L, do_Back_L_H,
		do_Back_L_M, };

uint8_t pwm_value_set(TIM_HandleTypeDef *htim, uint32_t Channel,
		uint32_t pwm_value)
{

	HAL_TIM_PWM_Stop(htim, Channel);
	TIM_OC_InitTypeDef sConfigOC = { 0 };
	sConfigOC.OCMode = TIM_OCMODE_PWM1;
	sConfigOC.Pulse = pwm_value;
	sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
	sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
	sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
	sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
	sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
	if (HAL_TIM_OC_ConfigChannel(htim, &sConfigOC, Channel) != HAL_OK) {
		Error_Handler();
	}
	HAL_TIM_PWM_Start(htim, Channel);
	return HAL_OK;
}
uint8_t creat_operate_data(uint8_t operate_type)
{
	now_operate.operation = ctrl_list[operate_type][OP_NAME];
	now_operate.wheel.left = ctrl_list[operate_type][OP_LEFT];
	now_operate.wheel.right = ctrl_list[operate_type][OP_RIGHT];
	return 0;
}

uint8_t pwm_value(double rx_pwm_mb, double rx_pwm_lr)
{
	double temp_mb_l = 0;
	double temp_mb_h = 0;
	double temp_lr_l = 0;
	double temp_lr_h = 0;

	temp_mb_l = DEAD_MB_L - rx_pwm_mb;
	temp_mb_h = rx_pwm_mb - DEAD_MB_H;
	temp_lr_l = DEAD_LR_L - rx_pwm_lr;
	temp_lr_h = rx_pwm_lr - DEAD_LR_H;

	if (rx_pwm_mb <= DEAD_MB_L) {
		if (rx_pwm_lr <= DEAD_LR_L) {
			/*前左*/
			if ((temp_mb_l - temp_lr_l) > DEAD_MID_LEN) {
				creat_operate_data(Go_L_L);
//				debug_printf("Go_L_L\r\n");
			} else if ((temp_lr_l - temp_mb_l) > DEAD_MID_LEN) {
				creat_operate_data(Go_L_H);
//				debug_printf("Go_L_H\r\n");
			} else {
				creat_operate_data(Go_L_M);
//				debug_printf("Go_L_M\r\n");
			}
		} else if (rx_pwm_lr >= DEAD_LR_H) {

			if ((temp_mb_l - temp_lr_h) > DEAD_MID_LEN) {
				creat_operate_data(Go_R_L);
//				debug_printf("Go_R_L\r\n");
			} else if ((temp_lr_h - temp_mb_l) > DEAD_MID_LEN) {
				creat_operate_data(Go_R_H);
//				debug_printf("Go_R_H\r\n");
			} else {
				creat_operate_data(Go_R_M);
//				debug_printf("Go_R_M\r\n");
			}
			/*前右*/
		} else {
			/*前*/
			creat_operate_data(Go);
//			debug_printf("Go\r\n");
		}
	} else if (rx_pwm_mb >= DEAD_MB_H) {
		if (rx_pwm_lr <= DEAD_LR_L) {
			/*后左*/
			if ((temp_mb_h - temp_lr_l) > DEAD_MID_LEN) {
				creat_operate_data(Back_L_L);
//				debug_printf("Back_L_L\r\n");
			} else if ((temp_lr_l - temp_mb_h) > DEAD_MID_LEN) {
				creat_operate_data(Back_L_H);
//				debug_printf("Back_L_H\r\n");
			} else {
				creat_operate_data(Back_L_M);
//				debug_printf("Back_L_M\r\n");
			}
		} else if (rx_pwm_lr >= DEAD_LR_H) {
			/*后右*/
			if ((temp_mb_h - temp_lr_h) > DEAD_MID_LEN) {
				creat_operate_data(Back_R_L);
//				debug_printf("Back_R_L\r\n");
			} else if ((temp_lr_h - temp_mb_h) > DEAD_MID_LEN) {
				creat_operate_data(Back_R_H);
//				debug_printf("Back_R_H\r\n");
			} else {
				creat_operate_data(Back_R_M);
//				debug_printf("Back_R_M\r\n");
			}
		} else {
			/*后*/
			creat_operate_data(Back);
//			debug_printf("Back\r\n");
		}
	} else {
		if (rx_pwm_lr <= DEAD_LR_L) {
			/*左 Left*/
			creat_operate_data(Left);
//			debug_printf("Left\r\n");
		} else if (rx_pwm_lr >= DEAD_LR_H) {
			/*右 Right*/
			creat_operate_data(Right);
//			debug_printf("Right\r\n");
		} else {
			/*停 stop*/
			creat_operate_data(Stop);
//			debug_printf("Stop\r\n");
		}
	}
	ch_pwm_value.ch_mb = rx_pwm_mb;
	ch_pwm_value.ch_lr = rx_pwm_lr;

	for (uint8_t i = 0; i < CTRL_LIST_NUM; i++) {
		if (i == now_operate.operation) {
			if (now_operate.operation != last_operate.operation) {
				debug_printf("--%d--\r\n", now_operate.operation);
			}
//			run_fun_list[i]();
			last_operate = now_operate;
		}
	}
	return 0;
}
void change_check(void)
{
	if (now_operate.wheel.left != last_operate.wheel.left) {
		pwm_value_set(&htim8, TIM_CHANNEL_2, STOP_SPEED);		//left
		pwm_value_set(&htim8, TIM_CHANNEL_1, STOP_SPEED);		//left
		osDelay(200);
	}
	if (now_operate.wheel.right != last_operate.wheel.right) {
		pwm_value_set(&htim8, TIM_CHANNEL_3, STOP_SPEED);		//right
		pwm_value_set(&htim8, TIM_CHANNEL_4, STOP_SPEED);		//right
		osDelay(200);
	}
}

void do_run(void)
{
	switch (now_operate.operation) {
		case Stop:
			do_Stop();
		break;
		case Go:
			do_Go();
		break;
		case Back:
			do_Back();
		break;
		case Left:
			do_Left();
		break;
		case Right:
			do_Right();
		break;
		case Go_R_L:
			do_Go_R_L();
		break;
		case Go_R_M:
			do_Go_R_M();
		break;
		case Go_R_H:
			do_Go_R_H();
		break;
		case Back_R_H:
			do_Back_R_H();
		break;
		case Back_R_M:
			do_Back_R_M();
		break;
		case Back_R_L:
			do_Back_R_L();
		break;
		case Back_L_L:
			do_Back_L_L();
		break;
		case Back_L_M:
			do_Back_L_M();
		break;
		case Back_L_H:
			do_Back_L_H();
		break;
		case Go_L_H:
			do_Go_L_H();
		break;
		case Go_L_M:
			do_Go_L_M();
		break;
		case Go_L_L:
			do_Go_L_L();
		break;
	}
}
uint8_t do_Stop(void)
{
	pwm_value_set(&htim8, TIM_CHANNEL_2, STOP_SPEED);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, STOP_SPEED);		//right
	pwm_value_set(&htim8, TIM_CHANNEL_1, STOP_SPEED);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, STOP_SPEED);		//right
	return 0;
}

uint8_t do_Go(void)
{
	double temp0 = 0;
	temp0 = (DEAD_MB_L - ch_pwm_value.ch_mb) / DEAD_MB_LEN;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_2, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, temp0);		//right
	return 0;
}
uint8_t do_Back(void)
{
	double temp0 = 0;
	temp0 = (ch_pwm_value.ch_mb - DEAD_MB_H) / DEAD_MB_LEN;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_1, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, temp0);		//right
	return 0;
}
uint8_t do_Left(void)
{
	double temp0 = 0;
	temp0 = (DEAD_LR_L - ch_pwm_value.ch_lr) / DEAD_LR_LEN;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_1, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, temp0);
	return 0;
}
uint8_t do_Right(void)
{
	double temp0 = 0;
	temp0 = (ch_pwm_value.ch_lr - DEAD_LR_H) / DEAD_LR_LEN;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_2, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, temp0);		//right
}

uint8_t do_Go_R_L(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;
	/*左轮*/
	temp0 = ((DEAD_MB_L - DEAD_MID_LEN) - ch_pwm_value.ch_mb)
			/ (DEAD_MB_LEN - DEAD_MID_LEN);
	temp2 = temp0;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;
	/*右轮*/
	temp1 = 1 - (ch_pwm_value.ch_lr - DEAD_LR_H) / (DEAD_LR_LEN - DEAD_MID_LEN);
	temp1 = temp1 * temp2;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	pwm_value_set(&htim8, TIM_CHANNEL_2, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, temp1);		//right
	return 0;
}
uint8_t do_Go_R_M(void)
{
	double temp0 = 0;
	/*左轮*/
	temp0 = (ch_pwm_value.ch_lr - DEAD_LR_H) / (DEAD_LR_LEN);
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;
	/*右轮*/

	pwm_value_set(&htim8, TIM_CHANNEL_2, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, STOP_SPEED);		//right
	pwm_value_set(&htim8, TIM_CHANNEL_4, STOP_SPEED);		//right
	return 0;
}
uint8_t do_Go_R_H(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;
	/*左轮*/
	temp0 = (ch_pwm_value.ch_lr - (DEAD_LR_H + DEAD_MID_LEN))
			/ (DEAD_LR_LEN - DEAD_MID_LEN);
	temp2 = temp0;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;
	/*右轮*/
	temp1 = 1 - (DEAD_MB_L - ch_pwm_value.ch_mb) / (DEAD_MB_LEN - DEAD_MID_LEN);
	temp1 = temp1 * temp2;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	pwm_value_set(&htim8, TIM_CHANNEL_2, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, temp1);		//right
	return 0;
}

uint8_t do_Back_R_H(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;

	/*右轮*/
	temp0 = (ch_pwm_value.ch_lr - (DEAD_LR_H + DEAD_MID_LEN))
			/ (DEAD_LR_LEN - DEAD_MID_LEN);
	temp2 = temp0;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	/*左轮*/
	temp1 = (ch_pwm_value.ch_mb - DEAD_MB_H) / (DEAD_MB_LEN - DEAD_MID_LEN);
	temp1 = temp1 * temp2;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	pwm_value_set(&htim8, TIM_CHANNEL_2, temp1);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, temp0);		//right
	return 0;
}

uint8_t do_Back_R_M(void)
{
	double temp0 = 0;
	temp0 = (ch_pwm_value.ch_lr - DEAD_LR_H) / (DEAD_LR_LEN);
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_1, STOP_SPEED);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_2, STOP_SPEED);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, temp0);		//right
	return 0;
}

uint8_t do_Back_R_L(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;

	/*右轮*/
	temp1 = (ch_pwm_value.ch_mb - (DEAD_MB_H + DEAD_MID_LEN))
			/ (DEAD_MB_LEN - DEAD_MID_LEN);
	temp2 = temp1;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	/*左轮*/
	temp0 = 1 - (ch_pwm_value.ch_lr - DEAD_LR_H) / (DEAD_LR_LEN - DEAD_MID_LEN);
	temp0 = temp0 * temp2;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_1, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, temp1);		//right
	return 0;
}

uint8_t do_Back_L_L(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;
	/*左轮*/
	temp0 = (ch_pwm_value.ch_mb - (DEAD_LR_H + DEAD_MID_LEN))
			/ (DEAD_MB_LEN - DEAD_MID_LEN);
	temp2 = temp0;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;
	/*右轮*/
	temp1 = 1 - (DEAD_LR_L - ch_pwm_value.ch_lr) / (DEAD_LR_LEN - DEAD_MID_LEN);
	temp1 = temp1 * temp2;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	pwm_value_set(&htim8, TIM_CHANNEL_1, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_4, temp1);		//right
	return 0;
}
uint8_t do_Back_L_M(void)
{
	double temp0 = 0;
	temp0 = (DEAD_LR_L - ch_pwm_value.ch_lr) / (DEAD_LR_LEN);
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_1, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, STOP_SPEED);		//right
	pwm_value_set(&htim8, TIM_CHANNEL_4, STOP_SPEED);		//right
	return 0;
}
uint8_t do_Back_L_H(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;
	/*左轮*/
	temp0 = (DEAD_LR_L - DEAD_MID_LEN - ch_pwm_value.ch_lr)
			/ (DEAD_LR_LEN - DEAD_MID_LEN);
	temp2 = temp0;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;
	/*右轮*/
	temp1 = 1 - (ch_pwm_value.ch_mb - DEAD_MB_H) / (DEAD_MB_LEN - DEAD_MID_LEN);
	temp1 = temp1 * temp2;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	pwm_value_set(&htim8, TIM_CHANNEL_1, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, temp1);		//right
	return 0;
}

uint8_t do_Go_L_H(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;
	/*右轮*/
	temp1 = (DEAD_LR_L - DEAD_MID_LEN - ch_pwm_value.ch_lr)
			/ (DEAD_LR_LEN - DEAD_MID_LEN);
	temp2 = temp1;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	/*左轮*/
	temp0 = 1 - (DEAD_MB_L - ch_pwm_value.ch_mb) / (DEAD_MB_LEN - DEAD_MID_LEN);
	temp0 = temp0 * temp2;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_1, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, temp1);		//right
	return 0;
}
uint8_t do_Go_L_M(void)
{
	double temp1 = 0;
	/*右轮*/
	temp1 = (DEAD_LR_L - ch_pwm_value.ch_lr) / (DEAD_LR_LEN);
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp1;

	pwm_value_set(&htim8, TIM_CHANNEL_1, STOP_SPEED);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_2, STOP_SPEED);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, temp1);		//right
	return 0;
}
uint8_t do_Go_L_L(void)
{
	double temp0 = 0;
	double temp1 = 0;
	double temp2 = 0;
	/*右轮*/
	temp1 = (DEAD_MB_L - DEAD_MID_LEN - ch_pwm_value.ch_mb)
			/ (DEAD_MB_LEN - DEAD_MID_LEN);
	temp2 = temp1;
	temp1 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	/*左轮*/
	temp0 = 1 - (DEAD_LR_L - ch_pwm_value.ch_lr) / (DEAD_LR_LEN - DEAD_MID_LEN);
	temp0 = temp0 * temp2;
	temp0 = DUTY_CYCLE_RANGE - DUTY_CYCLE_RANGE * temp0;

	pwm_value_set(&htim8, TIM_CHANNEL_2, temp0);		//left
	pwm_value_set(&htim8, TIM_CHANNEL_3, temp1);		//right
	return 0;
}
